#include "geobase.h"

namespace visualization
{

void Point::setCoordinates(double xx, double yy, double zz)
{
	x = xx; 
	y = yy;
	z = zz;
}

Point Point::operator=(visualization::Point pnt)
{
	x = pnt.x;
	y = pnt.y;
	z = pnt.z;

	return (*this);
}

Point Point::operator+(visualization::Point pnt)
{
	Point tmp;
	tmp.x = x + pnt.x;
	tmp.y = y + pnt.y;
	tmp.z = z + pnt.z;
	return tmp;
}

Point Point::operator-(visualization::Point pnt)
{
	Point tmp;
	tmp.x = x - pnt.x;
	tmp.y = y - pnt.y;
	tmp.z = z - pnt.z;
	return tmp;
}

Point Point::operator*(double scale)
{
	Point tmp;
	tmp.x = x * scale;
	tmp.y = y * scale;
	tmp.z = z * scale;
	return tmp;
}

Point Point::operator/(double scale)
{
	if(abs(scale) < 1e-20)
	{
		cout << "Devided by zero at line " << __LINE__ << " of file " << __FILE__ << endl;
//		exit(-1);	
	}
	Point tmp;
	tmp.x = x / scale;
	tmp.y = y / scale;
	tmp.z = z / scale;
	return tmp;
}

Point &Point::operator+=(Point &pnt)
{
	x += pnt.x;
	y += pnt.y;
	z += pnt.z;
	return *this;
}

Point &Point::operator-=(Point &pnt)
{
	x -= pnt.x;
	y -= pnt.y;
	z -= pnt.z;
	return *this;
}
Point &Point::operator*=(double scale)
{
	x *= scale;
	y *= scale;
	z *= scale;
	return *this;
}
Point &Point::operator/=(double scale)
{
	if( abs(scale) < 1e-300 )
	{
		cout << "Devided by zero at line " << __LINE__ << " of file " << __FILE__ << endl;
		//exit(-1);
	}
	x /= scale;
	y /= scale;
	z /= scale;
	return *this;
}

Point &Point::rotate(double thita)
{
	double Cos = cos(thita), Sin = sin(thita);
	double tmpX = x * Cos - y * Sin;
	double tmpY = x * Sin + y * Cos;
	x = tmpX;
	y = tmpY;
	return *this;
}

Point linearInterpolation(Point &highPnt, Point &lowPnt, double ratio)
{
	Point tmp;
	tmp.x = lowPnt.x * (1 - ratio) + highPnt.x * ratio;
	tmp.y = lowPnt.y * (1 - ratio) + highPnt.y * ratio;
	tmp.z = lowPnt.z * (1 - ratio) + highPnt.z * ratio;
	return tmp;
}

Point &Point::operator+=(const Vector &v)
{
	x += v.x;
	y += v.y;
	z += v.z;
	return *this;
}

Point &Point::operator-=(const Vector &v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
	return *this;
}

double Point::Distance(const Point &pnt)
{
	double xx, yy, zz;

	xx = x - pnt.x;
	yy = y - pnt.y;
	zz = z - pnt.z;

	return sqrt(xx*xx + yy*yy + zz*zz);
}

void Point::drawArrow(Point pos, Point dir, Point end[2])
{
	Point tmp = dir;
	double m = pow(dir.getX(), 2);
	m += pow(dir.getY(), 2);
	m += pow(dir.getZ(), 2);
	tmp /= sqrt(m);
	pos -= tmp;
	tmp /= 5;
	tmp.rotate(3.14/2);
	end[0] = pos + tmp;
	end[1] = pos - tmp;
}

Vector & Vector::operator+=(const Vector &v)
{
	x += v.x;
	y += v.y;
	z += v.z;
	return *this;
}

Vector & Vector::operator-=(const Vector &v)
{
	x -= v.x;
	y -= v.y;
	z -= v.z;
	return *this;
}

Vector & Vector::operator*=(double scale)
{
	x *= scale;
	y *= scale;
	z *= scale;
	return *this;
}

Vector & Vector::operator/=(double scale)
{
	x /= scale;
	y /= scale;
	z /= scale;
	return *this;
}

double Vector::Norm() const
{
	return sqrt(x*x + y*y + z*z);
}

void Vector::Normalize()
{
	double length = Norm();

	if (EQZero(length))
		return;

	x /= length;
	y /= length;
	z /= length;
}

void Vector::Cross(const Vector &rhs)
{
	double retx, rety, retz;

	retx = y*rhs.z - z*rhs.y;
	rety = z*rhs.x - x*rhs.z;
	retz = x*rhs.y - y*rhs.x;

	x = retx;
	y = rety;
	z = retz;
}

const Vector Vector::Cross(const Vector &lhs, const Vector &rhs)
{
	Vector ret(lhs);
	ret.Cross(rhs);
	return ret;
}

const Point operator+(const Point &lhs, const Vector &rhs)
{
	Point ret(lhs);
	ret += rhs;
	return ret;
}

const Point operator-(const Point &lhs, const Vector &rhs)
{
	Point ret(lhs);
	ret -= rhs;
	return ret;
}

const Vector operator-(const Point &lhs, const Point &rhs)
{
	return Vector(
		lhs.x - rhs.x,
		lhs.y - rhs.y,
		lhs.z - rhs.z);
}

const Vector operator+(const Vector &lhs, const Vector &rhs)
{
	Vector ret(lhs);
	ret += rhs;
	return ret;
}

const Vector operator-(const Vector &lhs, const Vector &rhs)
{
	Vector ret(lhs);
	ret -= rhs;
	return ret;
}

const Vector operator*(const Vector &lhs, double rhs)
{
	Vector ret(lhs);
	ret *= rhs;
	return ret;
}

const Vector operator/(const Vector &lhs, double rhs)
{
	Vector ret(lhs);
	ret /= rhs;
	return ret;
}

}

